/*
 * simulator.cpp
 *
 *  Created on: Jun 25, 2009
 *      Author: matt
 */

#include "simulator.h"
#include "algorithm"

SIMULATOR::SIMULATOR() {
	Init();
}

void SIMULATOR::ClearCollisionData() {
	bool laser = mode & LASER_COLLISION;
	if (!laser) {
		cdata.clines.clear();
		cdata.cpoints.clear();
		cdata.num = 0;
	} else {
		for (unsigned int i = 0; i < laserdata.size(); i++) {
			laserdata[i].clines.clear();
			laserdata[i].cpoints.clear();
			laserdata[i].num = 0;

		}
		laserdata.clear();
	}
}

SIMULATOR::~SIMULATOR() {
}

SIMULATOR::SIMULATOR(int mode) {
	this->mode = mode;
	Init();
}

bool SIMULATOR::Collision() {
	vector<LINE2DF> rhull;
	vector<LINE2DF> ehull;
	vector<LINE2DF> lasers;
	ClearCollisionData();
	env->GetHull(ehull);
	robot->GetHull(rhull);
	bool comprehensive = mode & COMPREHENSIVE_COLLISION;
	VECTOR2DF intersection;
	bool laser = mode & LASER_COLLISION;
	unsigned int size;
	if (laser) {
		size = robot->laser.size();
		robot->GetLaser(lasers);
		for (unsigned int i = 0; i < lasers.size(); i++) {
			COLLISION_DATA cd;
			cd.num = 0;
			laserdata.push_back(cd);
		}
	} else {
		size = rhull.size();
	}
	LINE2DF interl;
	LINE2DF envl;
	for (unsigned int i = 0; i < size; i++) {
		for (unsigned int j = 0; j < ehull.size(); j++) {
			envl = ehull[j];
			if (laser) {
				interl = lasers[i];
			} else {
				interl = rhull[i];
			}

			if (envl.intersect(interl, intersection)) {
				if (comprehensive) {
					if (!laser) {
						COLLISION_TUPLE ct = { interl, envl };
						cdata.clines.push_back(ct);
						bool found = false;
						for (unsigned int k = 0; k < cdata.num; k++) {
							if (dist(cdata.cpoints[k], intersection)
									<= COLLISION_TOLERANCE) {
								found = true;
								break;
							}
						}
						if (!found) {
							VECTOR2DF in = { intersection.x, intersection.y };
							cdata.cpoints.push_back(in);
							cdata.num++;
						}
					} else {
						COLLISION_TUPLE ct = { interl, envl };
						laserdata[i].clines.push_back(ct);
						bool found = false;
						for (unsigned int k = 0; k < laserdata[i].num; k++) {
							if (dist(laserdata[i].cpoints[k], intersection)
									<= COLLISION_TOLERANCE) {
								found = true;
								break;
							}
						}
						if (!found) {
							VECTOR2DF in = { intersection.x, intersection.y };
							laserdata[i].cpoints.push_back(in);
							laserdata[i].num++;
						}
					}
				} else
					return true;
			}
		}
	}
	if (laser) {
		for (unsigned int i = 0; i < laserdata.size(); i++) {
			if (laserdata[i].num > 0)
				return true;
		}
	} else {
		if (cdata.num > 0)
			return true;
	}
	return false;
}

void SIMULATOR::SetEnv(SIMULOBJECT * o) {
	env = o;
}

void SIMULATOR::SetRobot(ROBOT * o) {
	robot = o;
}

void SIMULATOR::Init() {
	//need to make sure that the modes are consistent
	if (mode & WALLDRAG)
		mode |= COMPREHENSIVE_COLLISION;
}

void SIMULATOR::RobotMoveForward() {
	if (mode & SIM_MOTOR_MODE)
		return;
	robot->SetResetPoint();
	float dx = -sin(robot->rrot) * MOVE_SPEED;
	float dy = cos(robot->rrot) * MOVE_SPEED;
	robot->Translate(dx, dy);
	if (Collision()) {
		robot->Reset();
		if (mode & WALLDRAG) {
			if (cdata.num == 1) {
				float fx = dx > 0 ? 1 : dx == 0 ? 0 : -1;
				float fy = dy > 0 ? 1 : dy == 0 ? 0 : -1;
				float u = cdata.clines[0].env_line.slope.x;
				float v = cdata.clines[0].env_line.slope.y;
				u = u >= 0 ? u : -1 * u;
				v = v >= 0 ? v : -1 * v;
				dx = fx * u * MOVE_SPEED;
				dy = fy * v * MOVE_SPEED;
				robot->Translate(dx, dy);
				if (Collision()) {
					if (cdata.num > 1)
						robot->Reset();
				}
			}
		}
	}
}

void SIMULATOR::SetMode(unsigned int mode) {
	this->mode = mode;
}

void SIMULATOR::RobotMoveBackward() {
	if (mode & SIM_MOTOR_MODE)
		return;
	robot->SetResetPoint();
	float dx = sin(robot->rrot) * MOVE_SPEED;
	float dy = -cos(robot->rrot) * MOVE_SPEED;
	robot->Translate(dx, dy);
	if (Collision()) {
		robot->Reset();
		if (mode & WALLDRAG) {
			if (cdata.num == 1) {
				float fx = dx > 0 ? 1 : dx == 0 ? 0 : -1;
				float fy = dy > 0 ? 1 : dy == 0 ? 0 : -1;
				float u = cdata.clines[0].env_line.slope.x;
				float v = cdata.clines[0].env_line.slope.y;
				u = u >= 0 ? u : -1 * u;
				v = v >= 0 ? v : -1 * v;
				dx = fx * u * MOVE_SPEED;
				dy = fy * v * MOVE_SPEED;
				robot->Translate(dx, dy);
				if (Collision()) {
					if (cdata.num > 1)
						robot->Reset();
				}
			}
		}
	}

}

void SIMULATOR::RobotTurnLeft() {
	if (mode & SIM_MOTOR_MODE)
		return;
	robot->SetResetPoint();
	float dangle = TURN_SPEED;
	robot->Rotate(dangle);
	//	bool comp_mode = false;
	//	if (mode & COMPREHENSIVE_COLLISION)
	//	{
	//		//this is slow so turn it off for now
	//		mode &= ~COMPREHENSIVE_COLLISION;
	//		comp_mode = true;
	//	}
	if (Collision())
		robot->Reset();
	//	if (comp_mode)
	//		mode |= COMPREHENSIVE_COLLISION;
}

void SIMULATOR::RobotTurnRight() {
	if (mode & SIM_MOTOR_MODE)
		return;
	robot->SetResetPoint();
	float dangle = -TURN_SPEED;
	robot->Rotate(dangle);
	//	bool comp_mode = false;
	//	if (mode & COMPREHENSIVE_COLLISION)
	//	{
	//		//this is slow so turn it off for now
	//		mode &= ~COMPREHENSIVE_COLLISION;
	//		comp_mode = true;
	//	}
	if (Collision())
		robot->Reset();
	//	if (comp_mode)
	//		mode |= COMPREHENSIVE_COLLISION;
}

void SIMULATOR::GetCollisionData(COLLISION_DATA & cd) {
	cd = cdata;
}

void SIMULATOR::RobotUseLaser() {
	mode |= LASER_COLLISION;
	robot->Laser();
	if (Collision()) {
		for (unsigned int i = 0; i < robot->laser.size(); i++) {
			float min_dist = 1e100;
			float min_index = 0;
			VECTOR2DF pos = { robot->xpos, robot->ypos };
			for (unsigned int k = 0; k < laserdata[i].cpoints.size(); k++) {
				float val = dist(laserdata[i].cpoints[k], pos);
				if (val < min_dist) {
					min_dist = val;
					min_index = k;
				}
			}
			robot->laser[i].length = min_dist;
		}
		ClearCollisionData();
	}
	mode &= ~LASER_COLLISION;
}

void SIMULATOR::SetMotorMode(bool enable) {
	if (enable) {
		//		POLARDCOORD2DF p;
		//		VECTOR2DF v =
		//		{ robot->xpos, robot->ypos };
		//		cartesian_to_polar(v, p);
		//		robot->polar_coords = p;
		//		VECTOR2DF xaxis = {1,0};
		//		VECTOR2DF nslope =
		//		{ cos(robot->rrot), sin(robot->rrot) };
		//		robot->curr_angle_with_horiz = angle(xaxis, nslope);
		mode |= SIM_MOTOR_MODE;
	} else
		mode &= ~SIM_MOTOR_MODE;
}

void SIMULATOR::Run() {
	if (mode & SIM_MOTOR_MODE) {
		float s1 = robot->speed[0];
		float s2 = robot->speed[1];
		if (s1 <= 0 || s2 <= 0) {

			return;
		}
		float ds = s2 - s1;
		if (ds != 0) {
			robot->SetResetPoint();
			float fx = ds < 0 ? -1 : 1;
			float fs = ds < 0 ? s1 : s2;
			ds = ds < 0 ? -ds : ds;
			float r = robot->axle_width * fs / ds;
			VECTOR2DF slope = { fx * (-cos(robot->rrot)), fx * (-sin(
					robot->rrot)) };
			VECTOR2DF nslope = { -slope.x, -slope.y };
			VECTOR2DF point_of_rotation = { robot->xpos + slope.x * r,
					robot->ypos + slope.y * r };
			float t = .1;
			float theta = fx * ds * t / robot->axle_width;
			VECTOR2DF unit = { 1, 0 };
			float ang = angle(unit, nslope);
			if (robot->ypos < point_of_rotation.y)
				ang = 2 * M_PI - ang;
			POLARDCOORD2DF p = { r, ang + theta };
			VECTOR2DF temp;
			polar_to_cartesian(p, temp);
			VECTOR2DF newpos = { point_of_rotation.x + temp.x,
					point_of_rotation.y + temp.y };
			normalize(temp);
			VECTOR2DF tangent = { -temp.y, temp.x }, ptangent = { -fx * sin(
					robot->rrot), fx * cos(robot->rrot) };
			robot->rrot += fx * angle(tangent, ptangent);
			robot->xpos = newpos.x;
			robot->ypos = newpos.y;
			if (Collision())
				robot->Reset();
		} else {
			if (robot->speed[0] != 0) {
				printf("Warning: both speeds set to same value.\n");
				mode &= ~SIM_MOTOR_MODE;
				RobotMoveForward();
				mode |= SIM_MOTOR_MODE;
			}
		}
	}

}

